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Abstract: This paper introduces several non-arbitrary feature selection techniques for a 
Simultaneous Localization and Mapping (SLAM) algorithm. The feature selection criteria 
are based on the determination of the most significant features from a SLAM convergence 
perspective. The SLAM algorithm implemented in this work is a sequential EKF (Extended 
Kalman filter) SLAM. The feature selection criteria are applied on the correction stage 
of the SLAM algorithm, restricting it to correct the SLAM algorithm with the most 
significant features. This restriction also causes a decrement in the processing time of the 
SLAM. Several experiments with a mobile robot are shown in this work. The experiments 
concern the map reconstruction and a comparison between the different proposed techniques 
performance. The experiments were carried out at an outdoor environment composed by 
trees, although the results shown herein are not restricted to a special type of features. 

Keywords: SLAM; mapping; features selection 



1. Introduction 

This paper addresses the problem of feature selection within a feature -based simultaneous localization 
and mapping (SLAM) algorithm. The feature selection methods shown herein are based on non-heuristic 
criteria in order to use only the most meaningful features according to the convergence theorem of the 
SLAM algorithm, in the correction stage of the SLAM. 
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The SLAM algorithm applied on a mobile robot recursively estimates the pose — localization and 
orientation — of the vehicle and the elements of the environment — called map — while reducing errors 
associated with the estimation process [1,2]. Several algorithms have been proposed as solutions to 
the SLAM problem. The most widely used by the scientific community is the Extended Kalman filter 
(EKF) [1,3-6] solution and its derived filters, such as the Unscented Kalman filter (UKF) [3] and the 
Extended Information filter (EIF) [7,8]. In these filters, the SLAM system state, composed by the robot's 
pose and the map of the environment, it is modeled as a Gaussian random variable. Others solutions has 
also been implemented to solve the SLAM problem with high success, such as the case of the Particle 
filter (PF) [9,10], the Graph-SLAM [1 1,12] and the FastSLAM presented in [3,13]. 

Different SLAM algorithms solutions are presented to solve one or several issues associated with 
the SLAM process, such as the time consuming processing, the accuracy of the map, the successful 
closure of the loop, the integration of the SLAM algorithm with control laws to drive the vehicle 
motion and the modeling of different environments (dynamic, highly dynamic, static, structured, 
unstructured, etc.) [2,3,5]. Thus, for example, the EKF-SLAM presented in [4] map lines extracted 
from structured environments whereas in [14,15] works on environments with point-based features 
(parameterized as range and bearing). The EKF has also been used in vision-based SLAM. Despite the 
easy implementation of the EKF-SLAM, the correction part of it demands high computation resources. 
To solve this, the EIF is used instead of the EKF [3]. The PF arises as an improvement of the 
map accuracy and makes the SLAM process independent from the Gaussianity restriction of the EKF, 
although its real time implementation jointly with non-reactive control laws is still in development. 

Several secondary process are involved within the SLAM algorithm, such as the case of the feature 
extraction process and the feature matching criterion. The feature extraction process determines the 
model associated with the environment and thus the map derived from the SLAM system state. The 
feature extraction procedure is also strongly related with the sensors incorporated on the mobile robot. 
Thus, for example, the line features or the point-based features mentioned before [4,14] are extracted by 
means of a range sensor laser, whereas the lines in [16] are extracted by a single camera. The feature 
extraction procedure is often a first environment filter of the SLAM. Those features whose quality is not 
acceptable for the mapping process or that have a certain probability of being a spurious measurement 
are rejected. The matching or data association is also crucial in the SLAM algorithm. A bad feature 
association could lead the SLAM to inconsistence [1,2]. Many feature association techniques have 
been proposed in the scientific literature, although the Mahalanobis distance is one of the most used 
criterion [3]. A successful matching will allow a successful SLAM. 

This paper introduces several non-heuristic criteria to select the most significant features from the 
environment to be used in the correction stage of the SLAM algorithm. The SLAM algorithm is 
implemented on an EKF. The selection criteria are based on the convergence theorem of the SLAM, 
restricting the correction stage of the estimation process to those features that contribute the most to 
the convergence of the determinant of the covariance matrix of the SLAM system state. Thus, four 
methods are presented: a first approach based on covariance ratio, a second approach based on the 
sum of the eigenvalues associated with the correction stage of the SLAM algorithm, a third approach 
based on the maximum eigenvalue also associated with the correction stage of the EKF-SLAM and 
a fourth approach based on the covariance matrices associated with the features extracted during the 
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feature extraction procedure. The optimization criteria and the corresponding algorithms for such 
feature selection procedures are also included in this work along with the appropriate extensions in 
the case that the covariance Joseph's form were used instead of the classical EKF covariance updating 
procedure [17]. Furthermore, the proposals are compared with a SLAM algorithm with an entropy-based 
feature selection and the full sequential EKF-SLAM [3]. Several experimental results and performance 
comparisons are also included in this work, showing the advantages of implementing a non-heuristic 
features selection method in a SLAM algorithm. Although the feature selection criteria presented herein 
are not restricted to the type of features used within the SLAM, an EKF-SLAM with point-based features 
is used to show the performance of each proposal. 

2. Related Work 

The need of selecting the features to be used by the SLAM algorithm is present at every SLAM 
algorithm design. The most common criterion is to select the best features from the feature extraction 
stage. Thus, those features without the quality demanded by the implementation would be rejected, 
such is the case shown in [18], where features that are not good enough for the mapping purpose are 
considered as spurious measurements. For example, in [4], the lines whose lengths are below a certain 
threshold are not added to the SLAM system state nor considered in the updating stage. 

Another example of the feature selection application within the SLAM algorithm is the one presented 
in [15,19]. When the SLAM is implemented on real time processes, the processing time becomes crucial 
for avoiding open loop situations [20]. According to this, for the purpose of reducing the processing time 
associated with the correction stage of the EKF-SLAM algorithm, a restriction is made on the number of 
features to be used during the updating. Thus, the work of [15,19] uses only a fixed number of features 
chosen regarding different criteria, such as proximity to the vehicle, smallest covariance associated with 
the extraction procedure or simply by the order in which the features were detected. 

On the other hand, the work of [21] presents a new criterion of chosen features according to the 
information provided by them to the SLAM algorithm. In order to do so, the entropy of the covariance 
matrix of the SLAM system state attached to each observed feature is calculated. If the information 
difference — see Section 4.1 — is over a certain threshold, then that feature will be used in the correction 
stage of the SLAM algorithm; otherwise it will be discarded. The main disadvantages of this method 
is the high computational time associated with the calculation of the determinant of the covariance 
matrix of the SLAM system state — which grows as the number of features increases — and the selection 
of the information difference threshold. This threshold represents a compromise in the design of the 
selection procedure. 

3. Sequential EKF-SLAM Algorithm 

The SLAM algorithm solved by an EKF is stated in Equation (1). All variables involved in the 
estimation process are considered as Gaussian random variables. 



Sensors 2011, 11 



65 



£" = /(&,«*) 

K t = P^Hj{H t P^Hf + R t )- 1 

i t = | t - + K t ( Zt - hdr)) 

P t = (I- K t H t )P t - 



(1) 



In Equation (1), is the predicted state of the system at time t; u t is the input control commands and 
it is the corrected state at time t; f describes the motion of the elements of £. Pf and P t are the predicted 
and corrected covariance matrices respectively at time t; A t is the Jacobian of / with respect to the 
SLAM system state and Q t is the covariance matrix of the noise associated to the process, whereas W t is 
its Jacobian matrix; K t is the Kalman gain at time t; H t is the Jacobian matrix of the measurement model 
(h) and R t is the covariance matrix of the actual measurement (z t ). The term (z t — to * s cai l e d the 
innovation vector [3] and takes place when the data association procedure has reached an appropriated 
matching between the observed feature and the predicted one (h(i^)). Both, the process model (/) and 
the observation model are non-linear expressions. Further information about the EKF-SLAM can be 
found in [22]. 

The sequential EKF-SLAM is based on the iterative calculation of the correction stage (SLAM system 
state and covariance matrix) for each feature with correct association — see [3]. The last statement implies 
that the Jacobian matrix of the measurement model and Kalman gain are sparse matrices, decreasing in 
that way the processing time during a correction iteration. Nevertheless, the prediction stage remains as 
stated in Equation (1). 

The general form of the correction stage of the classical sequential EKF-SLAM algorithm [3] is 
summarized in Algorithm 1. Sentences (3) to (9) describe the /or-loop of the correction stage of 
the algorithm. For every feature with correct association-sentence (2) — the /or-loop is executed. 
Sentence (4) shows the Kalman gain calculation; sentence (5) is the correction of the SLAM system 
state whereas sentence (6) is the correction of the covariance matrix of the SLAM algorithm; in 
sentence (7), the current feature is deleted from the set of features with correct association (M t ). In 
the next iteration, the next predicted SLAM system state and covariance matrix are the last corrected 
SLAM system state and covariance matrix respectively, as noted in sentence (8). 



Algorithm 1. Algorithm of the Sequential EKF-SLAM. 



2 



3 



Let N t be set of the observed features 

Let Mt C N t be the set of features with correct association 

for j = 1 to #M t do 



4: 



7 



5 



6: 



8 




9 



end for 
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4. Features Selection Criteria 

By exploiting the sequentiality condition of the EKF-SLAM presented in Algorithm I, the following 
sections will introduce several feature selection approaches for choosing the most significant features to 
be used in the correction stage of the SLAM from a non-arbitrary perspective. 

Thus, Section 4.1 shows a method for selecting features of the environment by means of the entropy 
associated with them; Section 4.2 shows the feature selection criterion based on the covariance ratio 
of the SLAM algorithm; Section 4.3 shows two selection criteria based on the eigenvalues associated 
with the covariance ratio of the SLAM algorithm; Section 4.4 shows the modifications of the previous 
feature selection criteria when the Joseph's form of the covariance matrix is used in the correction stage 
of the EKF-SLAM instead the one presented in Equation (1) and Section 4.5 shows the feature selection 
criteria based on covariance matrices associated with the features' extraction procedure. 

4.1. Features Selection: Entropy Approach 

The SLAM algorithm with feature selection based on the observation of the entropy of the 
measurements was previously presented by [21]. This algorithm is considered as related to the proposal 
herein. This method is based on the calculation of the entropy attached to each observed feature. If the 
entropy is below a certain threshold value, then the observation will be computed in the correction stage 
of the EKE. 

Considering that all variables involved in the EKF-SLAM estimation process are Gaussian random 
variables, the entropy value associated with a single observation can be represented as it is shown in 
Equation (2). 

E t = E-lnp(Ct\zi) = 0.51n[(2vre) n |Pt|] (2) 

In Equation (2), E is the entropy of the observation z. The a priori and posteriori information metric 
can be defined as the inverse of the entropy value, shown in Equation (2). Thus, 

z7n t = -E t = -0.51n[(27re) n |P t |] (3) 

im~ t = = -0.51n[(27re) n |P t -|] (4) 

The information difference can be calculated as in Equation (5), where the absolute incremental 
information is obtained. 

Aj = im t — im~[ (5) 

Thus, when the absolute information of a feature exceeds a certain threshold (5), that feature will be 
used in the correction stage of the EKF-SLAM algorithm. The algorithm of the EKF-SLAM with feature 
selection based on the entropy is summarized in Algorithm 2. 

As Algorithm 2 shows, the calculation of the entropy associated with a single observation — and 
its information metric — is related to the determinant of the complete covariance matrix of the SLAM 
system state. Thus, the complexity of the calculation of the entropy is 0(n 2 ), where n is the dimension 
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of the SLAM system state. Since this dimension varies, the complexity of the algorithm varies as 
well. Although this algorithm has the advantage of restricting the number of features to be updated, 
the calculation of the entropy requires the calculation of the determinant of the SLAM system state 
covariance matrix (P t ), which in fact increases the processing time of the EKF-SLAM algorithm. Further 
details on this approach can be found in [21]. 

Algorithm 2. Algorithm of the EKF-SLAM based on the entropy feature selection 
procedure. 

1: Let N t be set of the observed features at time t 
2: Let M t C N t be the set of features with correct association at 
time t 

3: V^i G M t ,P t \z{ is calculated according to Equation (1) 
4: if Aj > 5 then 

5: Update equation (£ 4 = <f 4 ~ + K t (z t — h^))) takes place. 
6: end if 

4.2. Features Selection: Covariance Ratio Approach 

The covariance ratio approach as feature selection criterion in the EKF-SLAM algorithm was formerly 
published by the authors in [23]. This approach is based on the evaluation of the influence of a given 
feature — with correct association — in the convergence of the covariance matrix of the SLAM system 
state. The correction of the covariance matrix of the SLAM system state can be expressed as: 

Pt = {I- K t H t )p- = P- - K t H t P t - 
Applying the determinant to both sides of the expression above, it follows: 

\P t \ = \I- K t H t \\P t -\ = \P- - K t H t p-\ (6) 

Considering that in Equation (6), Pf is pd (positive definite) and K t H t Pf is psd (positive 
semi-definite) matrices, the above expression leads to Equation (7). 

\P t \ = \I- K t H t \\p-\ = \P- - K t H t p-\ < \p-\ (7) 
Then, from Equations (6) and (7): 

ip I 

0 < ^-4- = | J - K t H t \ < 1; with \p-\ ^ 0 (8) 
\Pt I 

Equation (8) defines the covariance ratio of the SLAM algorithm. In this case, the ratio is used as 
a measure of the volume of the uncertainty ellipse associated with the covariance matrix of the SLAM 
system state [24] . 

Another convergence property states that, at the limit, all elements of P t become fully correlated [24] . 
This last statement is equivalent to say that, 



lim \P t \ = 0 

t— >oo 



(9) 
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Thus, according to Equations (8) and (9), given a set of observed features with correct matching, 
the feature that causes the highest decrease of \P t \, is the feature to which the EKF-SLAM is more 
sensitive to and will cause the fastest convergence of Equation (9). This latter point can be regarded as 
an optimization problem. Let N t be the set of observed features at time t; let M t C N t be set of features 
with correct association. Then \Jz e M t C N t : 



yOpt 



: arg z min(\Pt\) = arg z min(\I — K t H t \) 



(10) 

K t H t | is equivalent 



Thus, according to Equation (10), finding the observation z that minimizes \I 
to finding the observed feature that causes the highest decrease of \P t \ because \Pf\ is independent of 
the current observation. 

Considering that the EKF-SLAM implemented in this work is a sequential algorithm [3], the Jacobian 
of the observation model has the sparse form shown in Equation (11), where H v j is the Jacobian of 
the observation model with respect to the vehicle's degrees of freedom and H zt is the Jacobian of the 
observation model with respect to the parameters of the observed feature. 0i and 02 are null matrices. 
The Kalman — Equation (12) — gain is also defined according to Equation (11). 



H t = [H v>t 0i H z j 0 2 



(ID 



(12) 



Kj = [Kl t Kl K% Kl 2 ] 

Thus, the Jacobian of the observation is only calculated on the Jacobian entries that correspond to 
the vehicle and to the feature with correct association [1,2]. By using Equations (11) and (12), the 
determinant of \I — K t H t \ can be calculated as: 



1/ - K t H t 



Iv 









" K Utt ' 






Is, 


Iz 




K Ql 

K z>t 


[H v ,t ei H, tt e 2 ] 




Ie 2 . 
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Kv,tHv,t 
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-K Vl 


Hz,t 


e 




—Ke 1 Hv,t I&! 


—K@ 1 H Ztt 


e 




-k z , t H z , t 
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Iz — Kz,tHz,t 


e 






e 


-K Q2 H Ztt 


Ie 2 






Iv ~~ Kv,tHv,t 


-K v>t H ttt 






-Kz,tHv 


,t 


Iz ~~ Kz,tHz,t 





(13) 



In Equation (13), / is the identity matrix, I v , I z and Iq 2 are identity block matrices with the 
dimensions of K Vtt H v>t , Kq^i, K Zjt H Zjt and -Ke 2 ©2 respectively. If we consider that the vehicle has 
three degrees of freedom — two related to the position and one to the orientation — and the feature is 
determined by two parameters, then the final calculation of Equation (13) is a 5 x 5 matrix. 

The correction stage of the EKF-SLAM algorithm with the feature selection based on the covariance 
ratio is presented in Algorithm 3. 
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Algorithm 3. Algorithm of the EKF-SLAM based on the covariance ratio feature selection 
procedure. 



1 

2 
3 

4 
5 
6 
7 
8 
9 
10 
11 



Let N t be set of the observed features at time t 

Let M t C N t be the set of features with correct association at time t 

Let LIM be the maximum number of features to be used in the correction 

stage 

for j = 1 to min{LIM, #M t } do 

find z° pt : arg z min(\P t j\) = arg z min(\I — K t: jH t j\) 



K u = p-jH?.{H tJ p-.Hl 3 + R tJ ) 



p 



end for 



;3 t,J 

M t - {zf} 
= P*,i! £tj = 6 



-II -opt 



In Algorithm 3, sentences (1) — (2) are the declaration of the domain that is going to be used in the 
correction stage; sentence (3) determines — if possible — the maximum number of features that will be 
used for correcting the SLAM. If the number of features in M t is smaller than LIM, then the complete 
set of features in M t will be used in the correction loop. Sentences (4) — (9) show the /or-loop of 
the correction stage. Given M t , the algorithm searches for a first z opt . When it is found, the correction 
takes place — (6) to (8) — and this features is removed from M t . In the second iteration of the /or-loop, 
the z opt is searched inside the new M t and both the actual predicted system state and covariance are the 
last corrected system state and covariance matrix as shown in sentence (10). This situation ensures that 
sequentiality of the EKF-SLAM is not lost. 

4.3. Features Selection: Eigenvalues Approach 

In this approach, instead of selecting the features according to the determinant of Equation (13), the 
eigenvalues associated with it will be used. 

By inspection, it is possible to see that if an eigenvalue of Equation (13) tends to zero faster than 
the others, then that eigenvalue will dominate the convergence of \P t \ — see Equation (9). Thus, the 
eigenvalues approaches presented herein give a better description of the behavior of the set of eigenvalues 
associated with (/ — K t H t ) in Equation (13), because they consider the behavior of all eigenvalues. 

Let us calculate the eigenvalues of Equation (13) — Eig(I — K t H t ). Applying the definition of 
eigenvalues we have that: 



\{I - K t H t ) - \I\ 





e 


^1,3 


0 


A 2 ,i 


^2,2 


^2,3 


0 


^3,1 


e 


^3,3 


0 




e 


^4,3 


^4,4 



(14) 
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with, 



A l;i 
A 2 ,i 

A 2 ,2 
A 2 ,3 

A 3 ,x 
A 3 , 3 
A 4 ,i 
A 4 ,s 
A iA 



v,t 1J -v,t 



XL 



-K v , t Hz,t 
—Kq 1 H v ^ 



Jb 



A/ ei 



9i -oi 

-K Ql H z>t 
-K z , t H z>t 
I z - K z <H z t 



XI, 



—K Q2 H Vjt 
-Ke 2 H z , t 

I& n — XIpi 



Bo 



0 is a null matrix with the appropriate dimensions 

By inspection of Equation (14) and considering that 6 is a null matrix with the appropriate dimension, 
it is possible to see that, 



\{I - K t H t ) - XI\ 



xy 



(15) 



of 



Iy 1^11 t.Hi] 



Iv — Kv,tHv,t — XI V —K v jH z j 

—K zt H vt I z — K zt H zt — XI Z 

Thus, the only eigenvalues of (/ — K t H t ) affected by the current feature Zj are the eigenvalues 

v,t v,t z,t . The rest of the eigenvalues equal one — see Equation (15). 

K Zy tH v ,t Iz — Kz,tHz,t 

Considering that the pose of the robot has three degrees of freedom — two associated with the position 
and one with the orientation — and the feature has 2 parameters that define it, then the calculation of the 
eigenvalues of (/ — K t H t ) — which is an n x n matrix — is reduced to the calculation of a 5 x 5 matrix. 

In this section, two eigenvalues approaches are presented for selecting features. The first approach 
consists on choosing the features according with the sum of eigenvalues of Equation (15). Thus, from the 
set M t of features with appropriate association, only the feature with the minimum sum of its eigenvalues 
will be selected. 

The other approach is to select the features based on the lowest value of the highest eigenvalue. 
Equation (16) shows the selection criterion based on the sum of eigenvalues whereas Equation (17) 
shows the selection criterion based on the value of the highest eigenvalue associated with a feature. 



Vzi e M t , z°p 1 = arg z min(\P t \) 



Iv — Kv,tHv,t —Kv,tHz,t 
-K Ztt H Vjt I, - K Z}t H Z}t 



Vzi € M t , z opt = arg z min(\P t \) 



(16) 



)) 



arg z min(MAX Eig( 



-K Xtt H Vit I z - K z , t H z . t 



(17) 



)) 
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Algorithm 4. Algorithm of the EKF-SLAM based on the eigenvalues selection approach. 



1 

2 
3 

4 
5 
6 
7 
8 
9 
10 
11 



Let N t be set of the observed features at time t 

Let M t C N t be the set of features with correct association at time t 

Let LIM be the maximum number of features to be used in the correction 

stage 

for j = 1 to min{LIM, #M t } do 
find z° pt '■ a>rg z min(\Ptj\) 



-i\~°pt 



end for 



: (I - K t!j H t>j ))P t 

M t - {z%} ^ 



0 



— I opt 



Thus, in Equation (16), the feature selected has the minimum sum of eigenvalues; in Equation (17), 
the feature selected is the one which has the smallest maximum eigenvalue. The last is based on that 
if the higher eigenvalue decreases, also decrease (or remain equal) the rest of the eigenvalues. Thus, 
this method allows a selection of features based on the behavior of the eigenvalues. Further information 
about the sum of eigenvalues method can be found in [20]. 

Algorithm 4 shows the general structure of the selection procedure. Sentence (5) can be chose 
according to Equations (16) or (17). 

4.4. Features Selection: Joseph's Covariance Matrix Approach 

Up to now, the feature selection approaches presented are based on the covariance matrix of the 
SLAM system state: P t = (I — K t H t )Pf . Due to the fact of possible lost of positive definiteness of P t 
during the numerical computation, the Joseph's form of the covariance matrix of the SLAM system state 
within an EKF-SLAM is widely used by the scientific community [17]. The Joseph's form is shown in 
Equation (18). 



P t = (I - K t H t )P t -{I - K t H t ) T + K t R t K] 



(18) 



In Equation (18), R t is the covariance matrix of the observation. As it can be noted, the expression 
above corresponds to an n x n matrix, where n is the order of the SLAM system state. 

In order to reduce the computational cost by applying any selection criterion previously presented 
with Equation (18) instead of Equation (1) some calculations are needed. 

Thus, considering that (/ — K — tH t )Pf(I — K t H t ) T and K t RtKj in Equation (18) are psd the two 
following conditions hold. 



f \P t \ > \(I-K t H t )P t -(I-K t H t ) 
= II - K t H t \ 2 \P t \ 



(19) 



\P\> \ KtRtK? 
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In Equation (19), \Pf\ is a constant value because Pf is independent of the current feature; in 
addition, in Equation (19) \I — K t H t \ 2 < \I — K t H t \ < 1 according to Equation (8). 

The calculation of \I — K t H t \ applies as was shown in Equation (13). On the other hand, 
considering that K t R t Kj = Pf Hf% 1 R t ^ T H t Pf T , with ^ = H t PfHj + R t , then, it follows that 
\K t R t Kj\ = \Pf\ 2 \Hf^ 1 Rt9ft T H t \. Replacing M t = ^ x Rt^ T , where M has the dimensions of 
R t -2 x 2 matrix, then, 

\K t R t Kj\ = \P~\ 2 \HjM t H t \ 

Considering also that the EKF-SLAM used in this work is a sequential EKE, the above expression can 
be written as it is shown in Equation (20). 

HfM t H t = 



[Hl t 6f Hi &WM t [H v , t 0! H z>t 9 2 ] 

(20) 

H? tt M t H v , t 6 Hl t M t H z , e n 

e e e e 

H? jt M t H Vtt 9 H T Ztt M t H Z)t e 

e e e e 

In Equation (20), 9 means a null block matrix with the appropriate dimensions. By inspection of 
Equation (20) is possible to see that \P^ 2 \ \Hf M t H t \ = \K t R t Kj\ = 0 which leads to the following 
expressions. 



\P t -\\i - K t H t \ 2 < \p t \ < \P: 



I - K t H t \ 2 < Jgj- < l; with \Pr\ > 0 



(21) 



Thus, as it can be seen in Equation (21), smaller the determinant of 1 1 — K t H t \, smaller the value that 
| P t | could adopt. Re- writing Equation (21) follows that, 



1 < \H1 < i C22) 

- \Pt\ - \I-K t H t \* K > 

Equation (22) implies that the smaller \I — K t H t \ the bigger the inverse of the covariance ratio 
presented in Equation (8). Concluding, for a covariance matrix of the EKF-SLAM system state corrected 
according to the Joseph's form, the feature selection criterion is the same as the one presented in 
Equation (10): z opt : arg z min(\P t \) = arg z min(\I — K t H t \). From this last statement is possible 
to see that the feature selection approaches presented in Sections 4.2 and 4.3 apply in the same way 
when the Joseph's form of the covariance correction is used. 



4.5. Features Selection: Features' Covariance Approach 



The features' covariance approach is based on the following assumption. Let us suppose that at a 
time instant t, the mobile robot extracts five features from the environment (#iV t = 5 in Algorithm 1). 
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From the set of five features extracted, only two of them have appropriate association with the predicted 
features from the SLAM system state (#M t = 2 < #N t in Algorithm 1). Figure 1 shows this 
situation. Each of these two features has associated a covariance matrix (Ri and R 2 respectively) which 
intrinsically depends on the feature extraction procedure. Also, R± and R 2 are positive definite matrices. 
The features' covariance approach consists in choosing the feature with the smallest covariance matrix 
with respect to the covariance matrices of the rest of features with appropriate association. Thus, for 
example, in Figure I, if R\ is smaller than R 2 — (R 2 — -Ri) is positive semi-definite, then Feature 1 
will be used within the correction stage of the EKF-SLAM instead of Feature 2. Although it seems 
intuitive to choose -Ri — because of its smaller covariance, the following theorem is the corresponding 
mathematical justification of the features' covariance approach criterion. 

Figure 1. Graphic representation of two features with correct association (in red-star) among 
a set of five features extracted from the environment by the mobile robot's sensors. 




Mobile Robot 



Theorem — Let R x and R 2 be two symmetric positive definite covariance matrices associated with two 
features from the environment with correct association — as it is shown in Figure 1 . Also, let R 2 >z Ri 
(>z stands for positive semi-definite, therefore, R 2 — R\ y 0), then, 

\P t Rl \ <\Pt R2 \-~ (23) 

The theorem above establishes that the feature with associated covariance matrix Ri will cause the 
highest decrement of the uncertainty volume of the covariance matrix of the EKF-SLAM, \P t \, when 
compared with R 2 — see Equation (9). 

Proof — By hypothesis, we have that: 

Rx =< R 2 (24) 

Considering that H t P^ Hf in Equation (1) is positive semi-definite, then the above relation does not 
change if we add H t P^~ on both members of Equation (24). 



HtP-H? +R^ H t p-Hj + R 2 



(25) 
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Given that R\ and R 2 are positive definite (by hypothesis), then the matrix H t P t Hf + Ri, with 
% = 1, 2, is also positive definite. Therefore, its inverse exists. Then, 

(H t p-Hj + R 1 )- 1 y {H t p-Hj + RzY 1 (26) 

Equation (26) shows that, after applying the inverse at both sides of Equation (25) the positive 
definition between matrices changes [25]. Rewriting Equation (26) and pre-multiplying by P^Hf and 
post-multiplying by H t Pf respectively — observe that Pf Hf = (H t Pf) T — we have that, 

P t -H?((H t P t -H? + R 1 )~ 1 - (H t PfH? + R 1 + R^HtPf y 0 
P^Hj{H t P t -Hj + Rx^HtPT ~ P^Hj{H t P t -Hf + R 2 )- 1 H t Pf yO 

Observe that the matrices definition between Equations (26) and (27) is not lost (see [25]). Finally, 
adding and subtracting Pf in Equation (27) we have the following result. 

(Pf - p-Hj{H t p-Hj + R 2 r 1 H t P t -) - (P- - p-Hj{H t P t -Hj + R^H^) y 0 (28) 
Thus, according to Equations (1, 28) implies that, 

pR 2 _ pRj y Q 

where P^ 4 is the covariance matrix of the SLAM algorithm correction stage if the feature with covariance 
matrix Ri were used (i = 1, 2). 

Considering that both P^ 2 and P^ 1 are positive definite and Pf 1 ' 2 — P^ 1 y 0, then, according to [25], 
\P^ 2 \ > |P t Hl |. Therefore, having into account Equation (9), we can conclude that the feature that has 
the covariance matrix R\ associated with it will cause the highest decrement in the determinant of the 
covariance matrix of the SLAM system state correction stage. Then, that feature is the most meaningful 
from the convergence perspective of the SLAM algorithm. 

The Algorithm 5 presents the Features' Covariance approach for selecting the most meaningful 
features. The code line (6) shows the implementation of the selection criterion based on the covariance 
matrix of the features with correct association. In the case that a z opt is not found, then the correction 
of the EKF-SLAM is performed based on the detected features with correct association. Thus, i.e., if 
LIM = 2 and > 2 in the Algorithm 5 and no z opt exists, then the correction is performed with any 
two features from M t — code line (2) and (3). 

5. Experimental Results 

The mobile robot used in this work is a nonholonomic unicycle type Pioneer 3AT built by ActivMedia 
with a range sensor laser SICK incorporated on it. The laser acquires 181 measurements in a range 
of 30 meters, from 0 to 180 degrees. Figure 2 shows the mobile robot used as well as the SICK laser 
mounted on it. 

The feature extraction procedure was based on a clustering algorithm to extract point-based features, 
as the one shown in [14]. The parameters of the features were their range and bearing. The experiment 
were carried out outdoors and each detected feature was associated with a tree of the environment. The 
SLAM system state was composed by both: the vehicle degrees of freedom (position and orientation) and 
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the parameters of the features according with their extraction instant. Further and detailed information 
about the SLAM initialization, feature extraction and implementation issues can be found in [1,3,14]. 

Algorithm 5. Algorithm of the EKF-SLAM based on the features ' covariance selection 
procedure. 

1: Let N t be set of the observed features at time t 
2: Let M t C Nt be the set of features with correct association at time t 
3: Let LIM be the maximum number of features to be used in the correction 
stage 

4: for j = 1 to min{LIM, #M t } do 
5. Zjj -^t,j 

6: find z° pt : arg z min(\P tj j\) = arg z min((R t j)) 
7: K t , = PZHT( Htd P~HZ + Hrj) ; zf ' 

8: 0, t.j • AY,Uf - /m(, ; )) 

9: P t;j = (I - K t>j H t>j ))P t -\zf 

10: M t = M t - {zf} 

12: end for 



Figure 2. Picture of the mobile robot Pioneer 3AT used in this work. The range sensor laser 
is mounted on the vehicle. 




For the purpose of testing the algorithms proposed in Section 4 and see their differences and 
advantages, the following considerations must be taken into account: 

• The robot should navigate within the same environment 

• The robot should follow the same path within the environment in order to ensure that each SLAM 
algorithm with the corresponding feature selection criterion visits the same zones. 

In order to achieve such conditions, all the SLAM algorithms were implemented in parallel. The 
mobile robot followed a pre-established path by means of the Kanayama's trajectory controller [26]. 
The path was previously determined by a differential GPS (built by Novatel). Also, the positions of the 
trees within the environment were previously measured by the differential GPS. Considering that the 
differential GPS measurement had an absolute error of ±0.1 meters, those measurements were used to 
compare the SLAM localization and mapping results. The mobile robot pose information provided to the 
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controller was obtained from the fusion of odometry information with the differential GPS information 
(improving the odometry of the vehicle). Thus, no feedback is presented between the SLAM algorithms 
and the robot. Figure 3 shows the general architecture of the implemented system. 

Figure 3. General architecture of the system. The trajectory controller uses the odometry of 
the mobile robot as input. The SLAM algorithms work independently from each other and 
from the controller. 
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In this work, six different SLAM algorithms were implemented: the sequential EKF-SLAM shown 
in Algorithm 1, the entropy selection approach (see Algorithm 2), the covariance ratio approach (see 
Algorithm 3), the two feature selection eigenvalues approaches (see Algorithm 4 in Section 4.3) and the 
features' covariance approach (see Algorithm 5). 

5.1. SLAM Results 

Figure 4 shows two maps representation of the environment. Figure 4(a) shows the map reconstruction 
when there is no restriction on the number of features to be used during the update stage of the 
EKF-SLAM; on the other hand, Figure 4(b) shows the map reconstruction when the sequential 
EKF-SLAM updating stage is restricted to the two first detected features; Figure 4(c) shows a zoom 
of Figure 4(a) where it can be seen that each feature has associated a covariance ellipse. The features of 
the environment are represented by blue triangles; the path traveled by the robot is a solid black line and 
the path estimated by the SLAM is a solid magenta line. 

Figure 5 shows the map reconstruction of the environment based on the information provided by 
EKF-SLAM algorithm with entropy feature selection criterion shown in Algorithm 2. Figure 5(a) shows 
the map reconstruction when gate of the information difference was set to 5 = 0.2; on the other hand, 
Figure 5(b) shows the map reconstruction for a gate of the information difference of S = 0.4 (see the 
Algorithm 2). As it can be seen, the map reconstruction in Figure 5(b) is less precise than the one shown 
in Figure 5(a). 

The EKF-SLAM results based on the covariance ratio approach shown in Algorithm 3 are shown in 
Figure 6. In Figure 6(a), LIM — the maximum number of features to be selected — is set to LIM = 5. 
The last means that only the five most significant features from the covariance ratio approach point 
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of view will be selected for the correction stage of the EKF-SLAM. Figure 6(b) shows the case when 
LIM = 2. 

As it can be seen, the map constructed by the EKF-SLAM with the feature selection based on the 
covariance ratio approach (see Figure 6) is more similar to the one shown in Figure 4(a) than the map 
constructed by the EKF-SLAM with the entropy feature selection criterion (see Figure 5). 

Figure 4. Map reconstruction using the sequential EKF-SLAM presented in 
Algorithm 1 — without feature restriction, (a) Shows both: the map and the traveled 
path using the information processed by the sequential EKF-SLAM without restricting 
the number of features to be updated; (b) shows the sequential EKF-SLAM results after 
restricting the number of features to be updated to the first two features in being detected by 
the system; (c) it is a zoom to show the covariance ellipse associated with each feature of 
the environment. The path traveled by the mobile robot is represented in a solid black line 
and the path estimated by the SLAM in solid magenta; the features are represented by blue 
triangles and the ellipses of uncertainty associated with each feature are in solid red. 
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Figure 5. Map reconstruction of the environment using the entropy feature selection criterion 
on the EKF-SLAM. (a) Shows the map reconstruction when the information difference gate 
was set to 5 = 0.2; (b) shows the map reconstruction when the information difference gate 
was set to 5 = 0.4. As it can be seen, the map precision depends on the selection of the 
information difference gate, which in turn is related to the design of the algorithm. The path 
traveled by the mobile robot is represented in a solid black line and the path estimated by 
the SLAM in solid magenta; the features are represented by blue triangles and the ellipses of 
uncertainty associated with each feature are in solid red. 
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Figure 6. Map reconstruction of the environment using the covariance ratio feature 
selection criterion on the EKF-SLAM. (a) Shows the map reconstruction when the five most 
significant features — LIM = 5 in Algorithm 3 — were used for the correction stage of the 
EKF-SLAM; (b) shows the map reconstruction when LIM = 2. The path traveled by the 
mobile robot is represented in a solid black line and the path estimated by the SLAM in 
solid magenta; the features are represented by blue triangles and the ellipses of uncertainty 
associated with each feature are in solid red. 
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Figure 7 shows the map reconstruction based on the sum of eigenvalues feature selection criterion 
(shown in Algorithm 4). Figure 7(a) shows the case when LIM = 5 (the five most significant features 
are used in the correction stage of the EKF-SLAM); on the hand, in Figure 7(b), LIM = 2. 

As it can be seen, Figure 7(a) is very similar to Figure 4(a). 

Figure 7. Map reconstruction of the environment using the sum of eigenvalues feature 
selection criterion on the EKF-SLAM. (a) Shows the map reconstruction when the five most 
significant features — LIM = 5 in Algorithm 4 — were used for the correction stage of the 
EKF-SLAM; (b) shows the map reconstruction when LIM = 2. The path traveled by the 
mobile robot is represented in a solid black line and the path estimated by the SLAM in 
solid magenta; the features are represented by blue triangles and the ellipses of uncertainty 
associated with each feature are in solid red. 




Figure 8 shows the map reconstruction based on the EKF-SLAM with the maximum eigenvalue 
feature selection criterion presented en Section 4.3. Figure 8(a) shows the case when LIM = 5 and 
Figure 8(b) shows the case for LIM = 2. 

Finally, Figure 9 shows the map reconstruction based on the EKF-SLAM with the features' 
covariance approach. Figure 9(a) shows the case when LIM = 5 and Figure 9(b) shows the case 
for LIM = 2. As it can be seen, the results shown in Figure 9 are very similar to the ones shown in 
Figure 6. 

For the purpose of showing the performance of each SLAM algorithm, Table 1 shows the mean 
square error (MSE) between the pre-established path and the estimated path by the SLAM algorithms. 
The MSE associated with each algorithm was calculated point-to-point according to the data stored 
from the mobile robot pose SLAM estimation and the predefined path. In addition, Figure 10 shows 
the error evolution between the estimated path and the predefined one. As it can be seen, the full 
sequential EKF-SLAM shows the smallest error at all time whereas the EKF-SLAM with the feature 
selection criterion based on the covariance ratio shows the closest evolution with respect to the full 
sequential EKF-SLAM. Furthermore, the EKF-SLAM with the feature selection method based on the 
entropy approach shows the worst path between all the executions. Figure 1 1 shows a zoom of Figure 10. 
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Figure 8. Map reconstruction of the environment using the maximum eigenvalue feature 
selection criterion on the EKF-SLAM. (a) Shows the map reconstruction when the five most 
significant features — LIM = 5 in Algorithm 4 — were used for the correction stage of the 
EKF-SLAM; (b) shows the map reconstruction when LIM = 2. The path traveled by the 
mobile robot is represented in a solid black line and the path estimated by the SLAM in 
solid magenta; the features are represented by blue triangles and the ellipses of uncertainty 
associated with each feature are in solid red. 
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Figure 9. Map reconstruction of the environment using the features' covariance selection 
criterion on the EKF-SLAM. (a) Shows the map reconstruction when the five most 
significant features — LIM = 5 in Algorithm 5 — were used for the correction stage of the 
EKF-SLAM; (b) shows the map reconstruction when LIM = 2. The path traveled by the 
mobile robot is represented in a solid black line and the path estimated by the SLAM in 
solid magenta; the features are represented by blue triangles and the ellipses of uncertainty 
associated with each feature are in solid red. 
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Table 1. Mean Square Error associated with the different SLAM algorithms with feature 
selection criterion. 



SLAM algorithms 


MSE (m 2 ) 


Sequential EKF-SLAM (without feature selection) 


0.11 


Sequential EKF-SLAM (correcting with 2 features) 


1.14 


EKF-SLAM: entropy approach (LIM = 5) 


1.09 


EKF-SLAM: entropy approach (LIM = 2) 


2.11 


EKF-SLAM: covariance ratio approach (LIM = 5) 


0.16 


EKF-SLAM: covariance ratio approach (LIM = 2) 


0.46 


EKF-SLAM: sum of eigenvalues approach (LIM = 5) 


0.15 


EKF-SLAM: sum of eigenvalues approach (LIM = 2) 


1.14 


EKF-SLAM: maximum eigenvalue approach (LIM = 5) 


0.17 


EKF-SLAM: maximum eigenvalue approach (LIM = 2) 


1.06 


EKF-SLAM: features' covariance approach (LIM = 5) 


0.16 


EKF-SLAM: features' covariance approach (LIM = 2) 


0.63 



As it is shown in Table 1, when increasing the number of features to be used within the correction 
stage, decreases the MSE associated with the path estimated by the SLAM. Among the five feature 
selection approaches presented in this work, the feature selection criterion based on the covariance 
ratio approach has shown the best performance with both: LIM = 5 and LIM = 2. The entropy 
selection approach has shown to be the worst criterion given the experiment shown in Figure 5. 
Furthermore, the covariance ratio approach and both eigenvalues approaches have shown a better MSE 
when LIM = 2 than the sequential EKF-SLAM with the correction of the two first detected features (see 
Figure 4(b)). In addition, at the end of the experiments shown in Figures 4-9, the size of the map was 
of 50 point-based features for the full sequential EKF-SLAM — with no feature selection; for the 
EKF-SLAM with the entropy feature selection approach restricted to the two most significant 
features, the SLAM's map was of 150 features — mainly because of both: the bad association 
given by the Mahalanobis distance criterion used in this work [3] and the increasing processing 
time; the map obtained by the EKF-SLAM with the covariance ratio feature criterion approach 
restricted to two features was of 58 point-features whereas the map obtained by the EKF-SLAM 
with sum of eigenvalues and the maximum eigenvalue feature selection approaches restricted to 
two features was of 68 and 65 point-based features respectively. For the features' covariance 
approach, the number of features detected was of 60. As it can be seen, the EKF-SLAM 
with the covariance ratio feature selection approach and the features' covariance approach 
show the minimum map reconstruction when compared with the other methods with feature 
selection restriction. 



Sensors 2011, 11 



Figure 10. Evolution of the error of the different estimated paths by the EKF-SLAM 
algorithms proposed herein with respect to the predefined path. As it can be seen, the path 
estimated by the EKF-SLAM with the feature selection approach based on the covariance 
ratio shows the closest path to the one estimated by the full sequential EKF-SLAM. The path 
estimated by the entropy approach shows the worst path (with the higher error evolution). 
The Number of points axis refers to the successively points of the path used for the calculation 
of the error. 
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Figure 11. Zoom of Figure 10. As it can be seen, the paths estimated by the entropy approach 
is the worst estimated path. 
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The implementation results shown in Figures 6-8 were carried out using the Joseph's covariance 
matrix instead of the classical SLAM covariance matrix correction shown in Equation (1). The results 
of using the covariance matrix correction shown in Equation (1) have not shown significant map 
reconstruction differences with respect to the Joseph's approach. Hence, the graphical results were not 
included herein. 

5.2. Feature's Covariance Evolution 

For the purpose of showing the evolution of the features' estimation, Figures 12-17 show the evolution 
of the covariance of five features — with two parameters per feature — at different stages of the navigation 
of the experiments shown in Figures 4-9. Figure 12 shows the covariance evolution of the features 
when estimated by the full sequential EKF-SLAM without restrictions to the correction stage; Figure 13 
shows the evolution of the same set of features when estimated by the EKF-SLAM with the entropy 
feature selection approach; Figure 14 shows the evolution of the features when using the EKF-SLAM 
with the covariance ratio feature selection approach implemented; Figures 15 and 16 show the evolution 
of such set of features when estimated by the EKF-SLAM with the sum of eigenvalues and the maximum 
eigenvalue selection approaches respectively. Figure 17 shows the feature covariance evolution for the 
features' covariance approach. In Figures 13-17, the magenta feature and the green feature are not used 
in the correction stage. The last means that, although those features are added to the SLAM system 
state, they were not considered as most significant features at the moment of the correction of the SLAM 
algorithm. As it can be seen, there is no difference between the convergence of non- significant features 
compared with significant ones. 

Figure 12. Covariance evolution of a set of five features — with two parameters — according 
to the estimation carried out by the full EKF-SLAM without feature restriction. 
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Figure 13. Covariance evolution of a same set of features of Figure 12 estimated by 
EKF-SLAM with the entropy feature selection approach. 
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Figure 14. Covariance evolution of a same set of features of Figure 12 estimated by 
EKF-SLAM with the covariance ratio feature selection approach. 



„ 0.14 



0.12 



0.1 



a 0.08 



0.06 



o 0.04 



0.02 



Feature 
Feature 

- Feature 
Feature 
Feature 
Feature 
Feature 

- Feature 
Feature 
Feature 



1 (first parameter) 

1 (second parameter) 

2 (first parameter) 

2 (second parameter) 

3 (first parameter) 

3 (second parameter) 

4 (first parameter) 

4 (second parameter) 

5 (first parameter) 

5 (second parameter) 




500 1000 
Number of Iterations 



1500 



Figure 15. Covariance evolution of a same set of features of Figure 12 estimated by 
EKF-SLAM with the sum of eigenvalues feature selection approach. 



„ 0.14r 

c 

CD 

E 

§ 0.12 
: • 
ill 

a, 0.1 

si 

"o 

v> 0.08 

<D 
=3 

J 0.06 



- Feature 

- Feature 

- Feature 

- Feature 
Feature 
Feature 

- Feature 

- Feature 

- Feature 
Feature 



1 (first parameter) 

1 (second parameter) 

2 (first parameter) 

2 (second parameter) 

3 (first parameter) 

3 (second parameter) 

4 (first parameter) 

4 (second parameter) 

5 (first parameter) 

5 (second parameter) 



0.04- 



0.02 




500 1000 
Number of Iterations 



1500 



Sensors 2011, 11 



85 



Figure 16. Covariance evolution of a same set of features of Figure 12 estimated by 
EKF-SLAM with the maximum eigenvalue feature selection approach. 
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Figure 17. Covariance evolution of a same set of features of Figure 12 estimated by 
EKF-SLAM with the features' covariance selection approach. 
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5.3. Processing Time 



With the aim of showing the processing resources used by each EKF-SLAM with feature selection 
criterion algorithm, Figure 18 shows the accumulated processing time associated with them. Figure 18 
represents the amount of time that each algorithm of Table 1 remained processing data. As expected, the 
algorithms with a feature selection criterion have shown a lower accumulated processing time than the 
ones with non restrictions in the correction stage of the EKF-SLAM. Furthermore, the EKF-SLAM with 
the feature selection criterion based on the entropy (Section 4.1) has a bigger amount of processing time 
when compared with the others algorithms — with the exception of the sequential EKF-SLAM with non 
feature selection criterion. The increment on its accumulated processing time is due to the determinant 
of the covariance matrix of the SLAM system state, as stated in the information calculation shown in 
Equations (3) and (4). Thus, as increases the number of elements on the SLAM system state, increases 
the computational cost of calculating the determinant of the covariance matrix associated with it. 

The improvement of the restrictions within the sequential EKF-SLAM is linear with 0(n 2 ). 
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Figure 18. Accumulated processing time associated with each EKF-SLAM with feature 
selection criterion approach. 
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6. Conclusions 

This paper has presented several non-heuristic feature selection criteria for an EKF-SLAM algorithm. 
The feature selection criteria were based on the convergence theorem of the EKF-SLAM. Thus, only the 
features that cause the highest improvement in the convergence of the covariance matrix determinant of 
the SLAM system state were chosen for the correction stage of the algorithm. These features were called 
as the most significant features. 

Four feature selection approaches were shown in this work. The first approach consisted of selecting 
the most significant features based on the covariance ratio evaluation. The second approach was based 
on the sum of the eigenvalues and the third was based on maximum eigenvalue associated to (/ — K t H t ) 
in the correction stage of the EKF-SLAM. The fourth approach was based on the selection of features 
according to their covariance matrix and their meaning to the EKF-SLAM convergence. Furthermore, 
the feature selection proposals were extended for the case the Joseph's covariance correction form were 
used instead of the classical expression of the correction stage of the SLAM's covariance matrix. For all 
the approaches, the corresponding algorithms, the optimization criterion and the calculation reductions 
were also shown. 

Each EKF-SLAM with the feature selection criterion was compared with the sequential EKF-SLAM 
(where no feature selection restrictions were available), with a sequential EKF-SLAM where only the 
first two features were used in the correction stage and with a feature selection procedure based on the 
entropy associated with the covariance matrix of the SLAM algorithm. Several experimental results were 
also carried out, showing the performance of each feature selection proposal. 

Thus, for an outdoor environment composed by trees, the EKF-SLAM with feature selection criterion 
based on the covariance ratio and the features' covariance approach have shown a better performance 
than the rest of the feature selection criteria, showing the lowest mean square error of the traveled path 
when using only the two most meaningful features and the smallest processing time. On the other hand, 
the entropy based selection has shown the highest mean square error. Also, the EKF-SLAM with the 
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entropy based selection criterion was the algorithm with the highest computing demanding resources, 
mainly because of the determinant of the complete covariance matrix within its calculations. 

Despite of the fact that in this work the EKF-SLAM algorithm was based on point-based features, the 
selection criteria proposed herein are independent of the kind of features used. Furthermore, the feature 
selection criteria can be combined in order to robust the feature selection procedure. 
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